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SOME  REFINEMENTS  FOR  A SHIP  TRACKING  ALGORITHM 


INTRODUCTION 

Some  refinements  have  been  developed  for  the  ship  tracking 
algorithm  in  reference  1.  Also,  some  errors  in  this  reference  have 
been  discovered,  and  are  listed  in  Appendix  A here.  The  availability 
of  reference  1,  as  so  amended,  is  assumed  in  this  report. 

These  refinements  often  make  little  difference  when  the  input  posi- 
tion reports  of  the  ship  being  tracked  have  a large  margin  of  error. 

In  other  cases,  however,  the  difference  can  be  dramatic,  especially 
with  accurate  reports  followed  by  inaccurate  ones.  The  original 
algorithm  does  badly  in  such  cases  because  certain  pecularities  of  the 
underlying  ship  motion  model  cause  it  to  adhere  too  rigidly  to  the 
"average  velocity”  estimated  from  early  data.  The  refinements  correct 
this  tendency  by  adjusting  the  "state  covariance  matrix"  whenever  the 
estimated  "driving  noise"  increases.  This  adjustment  is  theoretically 
exact  for  the  case  of  only  two  position  reports  with  no  prior  velocity 
information. 

Upon  the  receipt  of  each  new  position  report,  the  original 
algorithm  operates  in  planar  coordinates  by  first  updating  an  estimate 
of  the  ship's  state  vector  (x,  y,  x,  y)  with  a Kalman  filter,  then 
updating  an  estimate  of  a 2 x 2 driving  noise  covariance  matrix  Q with 
the  "residuals"  from  this  filter.  This  Q matrix  estimate  is  then 
modified  to  make  it  diagonal  with  respect  to  the  current  estimate  of 
the  velocity  vector. 

One  refinement  consists  of  subsequently  updating  the  velocity 
components  of  the  state  covariance  matrix.  In  the  currently  estimated 
"in-track"  and  "cros|  track"  coordinates,  th^.s  (2x2)  submatrix  is 
increased  by  (t-t  ) Q,  where  each  term  in  Q is  the  greater  of  zero 
and  the  change  in  the  corresponding  term  of  Q from  its  preceding  value, 
and  where  t is  the  current  time  and  t is  the  time  that  tracking 
started. 

The  rationale  for  this  procedure  is  that  this  is  exactly  the 
correction  that  would  be  needed  in  the  state  covariance  matrix  after 
two  position  reports  if  this  matrix  were  generated  by  the  Kalman 
filter  using  too  low  a value  of  the  driving  noise.  In  a sense,  then, 
this  refinement  introduces  a degree  of  coordination  that  was  hitherto 
lacking  between  the  updating  of  the  Kalman  filter  and  of  the  Q matrix 
estimate. 


Note:  Manuscript  submitted  February  26,  1979. 
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For  good  results,  it  was  also  found  necessary  to  adopt  a more 
exact  procedure  for  diagonalizing  Q (in  the  currently  estimated  in-track 
and  cross-track  coordinates,)  than  was  used  in  the  original  algorithm. 

As  a practical  safeguard  against  extremely  oad  data,  it  was  found  wise 
to  limit  the  size  of  the  increment  in  the  velocity  covariance  submatrix 
to  that  submatrix's  initially  specified  value. 

The  details  of  these  refinements  are  specified  for  the  case  of 
planar  tracking  in  the  experimental  FORTRAN  implementation  of  Appendix 
B,  which  corresponds  to  the  program  listed  in  reference  1 (Figure  3,  pg. 
20-23)  for  the  original  algorithm.  The  section  "UPDATE  DRIVING  NOISE 
ESTIMATE"  is  replaced  entirely;  also  a few  bookkeeping  details  are 
added  in  the  "INITIALIZATION"  section.  These  refinements  extend  in 
a straightforward  way  to  the  algorithms  described  in  reference  1 for 
tracking  on  a sphere,  because  these  are  both  derived  from  the  planar 
tracking  procedure.  In  this  case,  however,  an  even  better  procedure, 
using  rhumb-line  track  projections,  is  described  in  reference  2. 

REFINEMENTS  FOR  PLANAR  TRACKING 


A.  New  Method  of  Recursively  Estimating  the  "Maneuvering  Intensity" 
Matrix  Q (a  2 x 2 matrix) 


V (p-  11  of 

tfhich 


approximate 


Instead  of  computing  the  statistics  4^,  ^ and 
reference  1),  we  compute  two  statistics,  3*jXand‘  ^4 _ , whi 
the  down-tr3ck  and  cross-track  components  of  the  "maneuvering  noise" 
intensity.  Assuming  that  these  two  components  are  uncorrelated,  the 
sort  of  reasoning  used  in  reference  1 to  derive  eqs.  (27) -(29)  leads 
to  a similar  recursive  scheme  for  generating  and  4C-  In  the  nota- 
tion of  reference  1,  this  scheme  consists  of  doing  the  following  at 
time  t, . , , after  updating  the  conditional  mean  and  covariance  matrix 


2 t . , , 

the^sta 


of  the  state  vector  according  to  eqs.  (8) -(24) 


(i)  Compute  the  down-track  and  cross-track  components  of  the 
"innovation"  vector 


h <i*1> 


Is 

To  do  this,  we  use  the  following  coordinate  transformation: 


2 


sin  e 


Vu~  + ❖“ 


a 

cos  o « 

'J777 

u,  v are  estimated 
velocity  components 


Hence,  we  have 


c,  =•  £ (i+1)  cos0  + e (i+1)  sine 
a x y 


c x 


■£  (i+1)  sine  + e (i+1)  cose 


(ii)  Compute  the  variance  of  £ and  £ , which  we  denote  by  b and 


The  covariance  matrix  of 

£ (i+1)' 

"b  !b 

X 

is 

xx  I xy 

e (i+1) 

b 'b 

L Y J 

L xy  i yy. 

where,  in  the  notation  of  reference  1 


+ + 2 4* 

b * p (t.)  + 2t  p (t.)  + t p (t.)  + r (i+1) 

XX  XX  1 XU  1 UU  1 XX 


p (t.)  + tp  (t.)  + tp  (t.)  + t"p  (t.)  + r (i+ 
xy  i xv  i ryu  i uv  l xy 


xy  * xy 

b = p (t+)  + 2t  p (t+)  » r“p  (t+)  » r (i+1). 
yy  *yy  i ryv  i i yy 

Hence,  from  the  coordinate  transformation, 


1)  and 
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b.  * b cos2^ 
d xx 

+ 2 

b 

xy 

sine 

cose 

w 2 

+ b sm  8 

yy 

2 

b = b sin  8 

C XX 

- 2 

b 

xy 

sine 

cose 

2 

+ b cos  8 . 

yy 

(iii)  Update  q.  and  q as  follows: 
d c 


‘l  -»* 


Si(i+1)  = qd(i)  +i+i[nax("*«ax'  T_)  - qd(i)I 


1 e2-b 

qc(i+1)  = qc(i)  +i~l[max(‘emax'  - qc(i)1' 


12  1 

where  e is  0 or  the  greatest  positive  value  of  either  — (e  -b.)  or  ~ 
max  t a a — c 

(e2-b  ) generated  in  any  iteration  so  far  (including  this  one) . The 

use  of  e keeps  q,  and  q from  occasionalv  achieving  unrealistically 
_ max . _ d c 

large  negative  values. 


This  recursion  scheme  is  carried  out  for  i -0  (i.e.,  after  the 

second  observation,  because  the  first  is  z in  the  notation  of 

reference  1).  It  is  initiated  by  setting  q. (0)  = q (0)  = e =0. 

a c max 

Until  the  second  observation  time  t, , we  use  Q = 0 (or  some  other 

selected  initial  value)  for  the  "maneuvering  matrix."  For  t in  the 

interval  (t^,  t^+^) , we  use  an  approximation  for  Q constructed  from 

q,  (i)  and  q (i)  as  follows: 
a c 


c 

c 


max [q^ ( i)  , 0] 

so  C » 

c | 0 

-+- 

max [5  (i),  0] 
c 

_°  lCc. 

positive-semidef inite . 


Q 


Sex  | Sey 

Sy  I Sy 


is  the  result  of  transforming  C from  down- 
track,  cross-track  coordinates  (as  currently 
estimated)  to  x,y  coordinates,  so 


Sex 

Sey 

‘Vy 


2.  2n 
c.  cos  9 + c sin  9 
d c 

(c.  - c ) sin9  cos9 
d c 

. 2 „ ^ 2 a 
c.  sm  9 + c cos  9. 
d c 
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B.  Updating  Velocity  Covariance  Submatrix  (2  x 2) 


Imnediately  after  updating  Q as  above  (say  at  t+,  i * 1) , we  also 
compensate  by  adjusting  the  submatrix  1 


(t*) 

! p (t+> 

1 

| UV  1 

■ — 

-f- 

. +. 

1 , + 

(ti> 

1 p (t . ) 

I W 1 

as  follows: 

(i)  Let  a^  and  a denote  the  values  of  c and  c obtained  on 
the  previous  iteration  (initially,  a,  = a = OT.  Then  compute 

u C 


d.  = min 
a 


[max(0,  c -a  J 1 

-v^J 


and 


max(0,  c -a  ) | 

2 = min  p ( t ) , — — 

c ruu  o t.-t  i 

. i o J 


d.  I 0 ’ 

? I** 


is  the  covariance  submatrix  increment  in  down-track,  cross- 

+.  1..2 


+ 12  + 

track  coordinates.  Also  note:  p (t  ) = -V  » p (t). 

uu  o 2 *vv  o 


(ii)  Transforming  the  increment  to  x,y  coordinates  gives  the 
updated  covariance  submatrix  components  as 


Puu(ti)  " puu(tl)  + ddCOs20  + dcsin20' 


p (t^)  = o (t*>  + (d  -d  ) sin9  cose,  and 

UV  X UV  1 u c 


Pw(ti)  = v11!1  + ddsin20  + dccos2e, 
where  is  used  as  in  FORTRAN. 


Summary 


Thus  the  "Final  Algorithm"  on  pp.  13-13  of  reference  1 is  changed 
as  follows: 

1.  , qxv  4..v  Ara  not  use^'  replaced  by  q^  and  q . 

Initial  values  are  q (0)  “ q (0)  “ 0.  Another  initial  value  is 
. d c 

e *■  0 . 
max 

1.  The  first  step  is  replaced  by  two  steps: 

• Jenerate  q , q , q ^for  ill,  these  are  trivially 

xx  xy  yy 

0 for  i ■ 0)  from  q^(i)  and  q (i)  as  described  above  in  (A). 

Also  update  e 

max 

f f ^ 

• Ad i us t p <t  ) , p it.)  and  p (t  ) as  described  in  above  m (B)  . 

uu  i uv  i w 1 

3.  The  next  two  steps  stay  the  same.  The  last  step  changes  to: 


,'ompute  q ( i + 1 ) and  q (i  + l>  as  described  above  in  (A) 

u C 


RHUMB -LINE  TRACK  PROJECTION 

The  preceding  modi: ications  for  planar  tracking  can  be  incorporated 
into  algorithms  for  tracking  on  a sphere  in  the  same  way  that  is  describ- 
ed in  reference  1.  Per  tracking  on  a sphere  with  geographical 
coordinates  (i.e«,  latitude  and  longitude),  however,  an  even  better 
procedure,  due  to  T.  3 . Bugenhagen  of  the  Johns  Hopkins  University 
Applied  Physics  Laboratory  [1],  is  to  propagate  the  estimated  target 
position  between  observations  along  a rhumb-line  instead  of  a great- 
circle  path,  again  by  dead  reckoning  with  the  estimated  average  velocity. 
This  modification  eliminates  the  need  for  coordinate  rotations,  and  is 
probably  a better  approximation  to  actual  ship  motion  anyway . The 
"track  propagation"  procedure  specified  on  pp.  35  and  3b  of  reference  1 
then  simplifies  to  the  following: 

• Beginning  at  time  t*  with  the  v axis  oriented  toward  local 
north  and  the  x axis  toward  local  east,  let  S . and  v be  the  latitude 
and  longitude  of  the  estimated  position  at  that  timeT  In  these  local 
rectangular  coordinates,  the  velocity  estimates  u,  , v , the  x 4) 
state  covariance  matrix  P.,  and  the  maneuvering  parameter  estimates 
q^(i)  and  q (i)  (see  part  A of  preceding  section'  are  also  available. 

For  convenience  here,  denote  south  latitudes  and  west  longitudes 
as  negative. 

e Propagate  the  ship's  position  by  dead  reckoning  to  the  time 

t , of  the  next  observation.  The  latitude  J1.  . and  longitude 
l + l i + l i+l 
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< 


of  this  dead-reckoned  position  can  be  computed  analytically  as  follows: 


f , - a + — ( t , - t ) : R ■ earth  radius 
i+l  i R i+l  i e 
e 

(unless  j i 4 *,  in  which  case  this  procedure  is  no  longer  valid.) 


A. 

Vi+l” 


a.  (t.  , - t. ) 

i i+i  i 

]b  + ■ - 

1 R COS'S  . 


if  v,  (t.  , - t ) “0 
i i+l  i 


(.  . r \ I \ 

u.\  cos^  \ l+sin$ 

\)ln  cosV  + 1 I * 1+sin*. 

• • 


otherwise 


• Compute  q^v  and  q from  q^(i)  and  q„(i)  as  in  part  A of 

the  preceding  section. 


• Compute  the  matrix  >T+^  from  eqs.  12  through  21  of  reference 


X(ti+1> 

K+i> 

ilt‘i+i> 

^Vi1 


values  at  time  t 

i+l 

in  local  coordinates 


• Perform  the  rest  of  the  procedure  as, in  reference  1,  except 
that  p and  q^(i+l)  and  q (i+l)  (instead  of  are  computed  as 

described  in  the  preceding  section  of  this  report. 
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ERRATA 

NRL  Report  7969 

Recursive  Filtering  Algorithms  for  Ship  Tracking 
Warren  W.  Willman 
April  6,  1976 

p.  10:  in  second  expression  from  bottom  should  be  v . . 

pp.  10  & 11:  m , m and  m should  be  replaced  by  the  respective 

xx  xy  yy 

expressions 

p (t+)  + 2p  (t*)  i + p (t+)  r2, 
xx  i rxu  l uu  i 

p (t*)  + ( p (t+)  + p (t+))i  + p (t+)  T"  and 
xy  i xv  i yu  i *uv  i 

2 

p (t , ) + .p  (t . ) T + p (t.)T. 
yy  i yv  i w i 

p.  11:  These  same  expressions,  with  the  index  i changed  to  j — 1 , should 

replace  p (t  ) , p (tj  and  p (t,),  respectively, 
xx  j xy  j yy  j 

q^d-1)  and  q^(i-l)  in  the  second  two  equations  should  be 
q^li+1)  and  (i+l) . 

p.  21:  Second  FORTRAN  statement  from  bottom  should  be  replaced  by 

the  four  statements 


RI  - FLOAT (I-l) 
GXX  - GXX-^xX*TAU 
GXY  - GXY -QXY*TAU 
GYY  • GYY-£YY*TAU 


(because  index  I starts 
at  1 instead  of  0) 


p.  25: 


The  coordinates  {?.  ,,  , ) at  the  end  of  line  7 should  be 

l+l  i+l 


-v 

(? 


tfi . Eq.  (51)  should  read  $ . 


i+l'  i+l 
v 

cosY  + — cos'?.  sinY)  . 
f i 


i+l 


sin  (sin? 


S 


Right  hand  side  of  equation  (A3b)  should  read 


v in  middle  alternative  of  (C9)  should  be  v.;  the  same  for 


in  last  equation  should  be  a 


o o <">  ooooooooono  o o o o r>  <-i  ooo 
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MODIFICATIONS  IN  PROGRAM  LISTING  OF  FIGURE  3 IN  NRL  REPORT  7969 


PROGRAM  TEST 

KALMAN  FILTER  WITH  AOAPTIVE  DA  IV ING  NOISE 

0 IN EN SION  TC999),XX(999) ,YY(999),XS(999),YS(999) .PXXC999) ,AX(999) 

01  MEN  SION  A»  (999),PXY(999),PYY(999),  ARXX(999),  AR X Y C 999 ) , AR Y Y ( 99  * ) 
DIMENSION  SNAJ(999),SNINC999),H(999) 

READ  PARAMETER  VALUES 

N 3 NUN3ER  OF  DETECTIONS  <3  NO.  OF  DATA  CAROS) 

VELVAP  3 PRIOR  SPEED  VARIANCE 

RE  AO  5071, N, VELVAR 
5071  FORMA  TC 13 ,F10«5) 

READ  (ANO  STORE)  DATA  FOR  EACH  0ETECTI3N 


T 3 TINE 

AX, AT  = OBSERVED  LOCATION  COORDINATES 

S* A = SEMirAJOR  AXIS  OF  86  PERCENT  CONTAINMENT  ELLIPSE 
FOR  OBSERVATION 

S“I  * SENIMINOR  AXIS  OF  CONTAINMcNT  ELLIPSE 
THT  3 ORIENTATION  0=  SEMIMAJOR  AXIS  (DEGREES  CLOCKWISE 
FROM  Y-AXIS) 

0 FORMA  K6F10.9) 

00  9 I - 1 , N 

READ  5070,T(I),AX(I),AT(I),SMA,SMI,THT 
THT*THT/57. 3 

ARXX(I)=((SNA*SIN(HT))»*<'»(S,NI*C0S(THT))«*2)/4. 

ARXYC I) -SIN(7HT)*C0S( TH  T ) *<  S*«A*SMA-SMI  *SMI  ) / 

9 ARYY(  I)  3C  (SMI  «SIN(  THT  ))**2U  SM  A*C  0S(  THT  ) ) **  2 )/ 4. 

INIT IAL II  AT  ION 


XX<  1)  3 A X(  l ) 

YY(  1) = AY C l ) 

PXX(1 ) 3ARXX ( I ) 

PXYCl )=ARXY( 1) 

P Y Y ( I )*ARYYv 1 ) 

Cl =PXX(1  )*PYY(1) 

C2»SDRT(CPXX(1)-PYY(1))**^»4.*PXY(1)**2) 

li  3 . > *(  ci  »c: ) 

C2=C1 -C2 

SMAJl 1)*2.pSJRT(CI) 

SNTNC 1 ) 3 2 . * SCRTC  C2 ) 
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I HP*  TU),Nt<0.)  M n 
Thl  1)  •'). 

<vx  t **  m 

r \ T iK  l > * *•  f . t»  A T ANx  l I* XXI  l ' - Cl 
M ONl'NlIl 
*M  •«'. 

Ml  • 0. 

‘.1*0. 

V*t>. 

f xu«o . 

I'XV*  ' . 

*» V «l-  ' . 

I»t  V » ) . 

(Mill*.  S * VF l y AK 

i»yv  * < mi 

jxx  • ' . 

iJXt  «.l  . 

Qtt*J. 

ci*o. 

cc*o. 

F NAX-l). 

«!*0. 

»»«  l N1  1C 
►'  *»  I N t 11 

10  P>N*NAHM)X,l'»Ml>«t  M .T„l)  P PS  111 XN S , / / ) 

11  *fl»»NX  11  10  X * ftfxlM  Xt  X X SNAJ  XX  S'* 

UN  Ax  X|lt  N1  »/  / ) 

H ' l . IH  S l V 1 SlAII  VfUflH  tsIMAUPN 

i'41  i I * .*  • y 

ix*axi u 

it  • At  1 1 ) 

*xx-» axxi  n 
kxtukxu i > 

ATt  «AX  t Y l 1 ) 

I All*  I il)«UCI) 

XXAK*  XXI l-t ) MI*T  All 
THAU*  tt(  l - 1 ) ♦ V * T A I 

C.XX  XX  l l - 1 >♦.'  . «I*XU*»  AJ»‘*UU*T  Ail*  T AH  *'4XX*  I All 
r.Xt -PXtv  t - l >*U*XV*f»Til)*T  AU*«MIV*T  AU*l  AlJ*<iXt  *!  All 
r.tt .m  tti  t - 1 > ♦ i'ty * i A0*“y v*  t aii*  i Au*idt t * i mi 

HXll  •*'  XU*»*U1I*  I All 
r.xv- |*XV*PUV*  1 VII 
C.Y  J**1  HIM’UV*  1 All 

i.tv-*ty*‘*yy  ♦ tau 

lliKA'Nt  •AFOU-teO  HIVIMJN  .Mil  “ill 

1 • iVVSMVATlXN  UIH 
T • TlNF 

XXX|tt)  . lllAA'NI  MUUlUl)  “YSUMN  IN  X't  uVM,'U*lli 
A»AJ  • SMUAJAA  AXIS  V !l.s  I'MClNI  w IN  I A l N*  l N 1 flip's:  '1 

CJAAlNT  "JSHPN 

X*  I N ■ SlMMNCiA  AXIS  9F  I'-.XNt  AINNkNt  rillfSI 

T't  • X A 1 1 N T A 1 1 X N V slMtXAJXA  AXIS  CltCXWlSl  MNA  I AXIS' 


nnn 


j 


Cl  «GXX»GYY 

C.!«SJRTUGXX-GYY)*»2»*.*GXY«GXY) 

Cl«.s*<Cl *CJ) 

c*  *u  -c^ 

S*4J(  I)«MV)#T(Cn 
S*IN(  l )««,.*iORT(C2  ) 

YP(&<  Y.NE.C.  1 r.  0 TO  fj 
TMC  I)  *0  . 

GO  TO  SO 

TO  THE  t>  «s  7.  I*  4 T AMCC  GK<  - Cl  >/0XY  )♦>>  J. 

50  PUNT  T,TCI),XBAR,YUR,S'*AJ(I>,SNINCI>iTh<I> 

C 

oe  t«c gxx»rxx)*(gyymyy)-cg*y  ♦rxy)*»j 

OETI«0. 

tP(0cT.GT.0.Y  OFTI.l./OET 

HXX. ( GYYMYY  )*0E  T l 
MXY*-(GXY»RXY)*r)FTT 
MYV.(GXXMXX)*PETI 

PXX(  t ) *GXX-GXX»GXX»HXX*2 .*GXX*CXY *HX Y -G XX •& XY  *HY Y 
P XY  ( I )*GXY-GXX*GXY*HXX-<  GXX» GY Y *GX Y*GX Y ) *HX Y-GX Y *G Y Y «M  YY 
PYT(T  )*GYT-GYX*GYT»iYY-J  . » G Y Y • G X Y ♦*<  X Y -C  X Y ♦ G X Y*M  X X 
Cl *PXX( I)»PYY(I) 

c*«s,rtupxxci)-pyyu  )>••;»«.  *pxyo  )**n 

Cl*»‘>*(.Cl*Cl  ) 

C2 *C1 -Cl 

S p AJl  I W.*SQRT(Cl  > 

$*:ni  t w.*:»qrtcc2) 

IF(P<  Y(  D.NE.O.)  i .7  YU  t; 

THE  I)  ->0. 

GO  TO  5 l 

T!  TM(  T)  '47.  )• ATANEE  »X<E I )-Cl )/®XY( [ ) )*»v. 

S 2 CONTINUE 

PXU«r.XU-GXX*GXU»HXX-CGXX»GYU»GXY*GXU)*HXY-rxY*GYU*rtYY 
PXV«GXV-GXX*GXV*HXX-( uXX»GYV*GXY*GXV)»HXY-GXY*GYV*MYY 

pyii«syu-gyy»gyu«hyy-e  ;rr  *gx'j»gxy  »gyu)  »hxy-gxy*gxu*hxx 

PYV«GYV-GYY»GYV»MYY-CGYY«GXV»GXY*GY V)«HXY-CXY*GXV*HXX 
PUU«P  UU-GXU*GXU*NXX . *gxu*gyu*hxy-gyu*gyu*myy 
PUV*PUy*r.XI)*GXV«HXX  - l iXl)*GYV*GYU*GXV  )«MXY-GYU*GYV*rtYY 
PVV'PVV-GXV*GXV*HXX-2.*GXV*GYV*HXY-GYV*GYY*MYY 
OET*PXX»RYY-RXY»RXY 
GE  TI«  0 • 

tF<0f T.GT.O.)  DETI'l./OFT 
HXX*RYY»06T  l 

HXY. -RXY*D€  TI 
MYY»AXX*0;T  l 

XXC  I)  *XB4R»EPXX(I)»HXX»®XYEI4»MXY  )»UX-XI14R) 

XX<  t)«XX(  n*(PXX<t  )*NXY*PXYcl)*HYY)*CY-Y34R) 
YYEI>'YBM*EPXYEI>MX<*»YYEI>*HXY)*EIX-XM«) 

YY( I) «YY<  n*(PXYCr)»HXY»PYY(I)*NYY)*<iY-Y4AR) 
U«I)»XPTU*MXX'PYU*MXY)«E2X-TGAR)*EPXU*HXY*PYU*MYY)*E2Y-Y94R) 
y»V»(PXV*NXX*PYV*'»XY)*(M-Y'lAR)KPXV*HXY*'PYy»MYY)»UY-Y4A*> 

UPOAT6  OR  iy  ING  NOISE  ESTIMATE  ANO  VELOCITY  COVARtANCE  SUSP  A T XIX 

I*(nu.lE.O.  ) GO  TO  l 
R!«RI* l • 
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noonooonono 


UC«MC 
Pl«0. 
p C « P 

SOINV.Pt 
U2«U*IJ 
V/ » V*  V 
UV ’U» V 

sg«soPt(U2*vj) 

TF(S,.GT.j.)  SCINV.I./Sj 
01NV»  SJINVPS'JINV 
tx.lt-x BAT 
E YWY-YUAR 

SIT*(U»CX*V»EY)*SJINY 

SCT»(U*tY-V«EX)«SJlNV 

r,xx*r  xx-oxx*tau»rxx 

GXY«r.XY-  JXY*  TAU»RXT 

gyy«gyy-;yy»  TAU^RYY 

GIT '*<Uc*GXX*2.*UV»GXY  » V 2 • G Y Y T * 0 I N V 
r,CT«C  V2*GXX-2.*UV*<'.XY  M2  *GYY  ) • 01 NV 
EC  I *C >n*SIT-GIT>/TAJ 
ECC  *< SCT*sCT-GCT)/TAJ 
IF<SC I.GT.E'AX)  5>«AX*EC  I 
IFCECC.GT.FNAX)  E1AX*ECC 
IFCECI.LT.-ENAX)  EC:*-EXAX 

ifcecc.lt. -erax)  ec:«-eyax 
Cl*CT  ♦CECt-CD/Rl 
CC*C:»<ECC-CC)/Rt 
H T *0  . 

*C  >0. 

TFCCT.GT.O.)  MI  * C I 
IFCCC.GT.3.  ) MC*C: 

QXXi(U2*MI»Y2pHC)*UNV 

0XY»uY*(MI-MC)*0ISV 

0TY*CV2*MI*U2yMC ) «01N  V 

DT*MT-0T 

0C*MC-0C 

I'CDl .GT.O.  ) PI«OI/(T(I)-TCl)) 

TFCOC.gT.3.)  PC«0C/CTC  T)-TU>> 

IFCPI.GT..S»VELVA«)  »I«.S*VeLVA< 

IFCPC .GT. .5* VELVA* ) PC«.X*VELVA» 

PUU«PUtl»CU2*PT*V2*PC>  «0TNV 
PUV*oUV*iJV*CPI-PC  >*  JlNV 
PVV .p VV*CV2»PI»J2»PC)»3INV 
1 CONTINUE 

THACXFC  OUTPUT 

I « OB  SEP  VAT  I ON  mex 
T . TI*F 

CXX,  YY)  * CURRFNT  POSITION  ESTIMATE  IN  X-Y  COO«JlNAYES 
S*  4 J • SSNlMAJfl*  A X t S OF  Bfc  PCACENI  CONTAINMENT  tLLlPSS  «0< 
CURRENT  POSITION 

S*  I N » SCIMNOP  AXIS  OP  CONIAINNFNT  p.LIPSF 

TH  • ORIENTATION  SEN  ;.N  AJO#  AXIS  CJEG.  CLOCKWISE  rHO*  Y-AXIST 
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print  a 

PRINT  12 
PRINT  u 
Off  3 1*1, N 

3 PRINT  7,  T(  I)  , XXC  t),TT(  I)  ,SrAJC  I)  ,SNIN(.  r)  , THC  I) 
PRINT  9 

7 F0RNATC25X ,F 10.2 ,SX,2  = 10. 2,bX,3c  10.2,/) 

9 F7R-ATC20X,///) 

12  FORMfcTCSOX ,17HTR4;<E)  POSITIONS,//) 

TRACE  S"30THER 


X SCN) *XXC V) 

7 SCN) *YY(N) 

N Nl  -*N  - l 
QEN»T(N)-T( l ) 
yXX*:xX*P'JU*  OEN 
QXY *0  X Y ♦ PUV  * 0 EN 
UYY  * }YY  *PVV*OcN 
DO  2 K=l,N*l 
I *N-< 

TAU*T(I»1  >-TC I) 

P1*PXXCI) 

P2  * PX TCI) 

P3*P* YC I) 

DEN«0»10XX*TAU>*(P)*  iYY»TAU)-(P2»QXY«T4U)«*2 
OEN  f«  0. 

IFCD'N.GT.O.)  DENI -l ./OEN 
MXX*Pl*CP3» JYY»T40)-»2*(»2»JXY*TAU) 

HXY*»>2*CP1  »QXY*TAJ)-P1  »CP2  ♦QXY*T  AU) 
HYX«P2*<P3»,Yt*TAJ)-»)*(P2*.XY*TAU) 

HYY  *p  3*(PU3XX*TAU)-92*<P2^)XY*T  \U  ) 

XS(I)*XXC  I )*(HXX*(XS(  I*l)-XXU)-'J*TAU)*HXY*(YS<I+l)-YYCI)-V«TAU))  • 
10ENI 

2 YSC I)  *YY(  I)*(HYX*(  XSC  I»1  )-XX(I)-0*TAU)»HYY*(YS< I*1)-YYCI)-V*T AU) )• 
1DENI 

SXX*PXXCN) 

SXYxPX Y(N) 

SY  Y =P  Y Y ( N ) 

Cl*SXX»SYY 

C2*Si.RTCC  SXX-SYY)  •*!  *<*  . » S X Y*  • 2 ) 

Cl*.5*CCl»C2) 

C2*Cl-C2 

SNAJC N) * 2. *S  QRT ( Cl ) 

SO  INC  N ) *2 .»  S0RTCC2  ) 

IFCSXY .NE.O.  ) 00  T3  73 
TMCN)*0. 

Off  TO  53 

7 3 THC  N)  *5  7.3»ATANCCSX<-:i  )/SYY)*9i). 

53  CONTINUE 
Off  5 1*2, N 
I *N“L  P 1 

T AU*T  C !♦ I )-T  C I) 
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Pl=PXX(I) 

P2=PYY(I) 

P3=PYY(I) 

41 =P1  ♦ !JXX*TAU 
A2=P2*QXY*TAU 
4 3=  P 3 4QY Y »T  A 'J 
DET=A1*A3-A2*A2 
OETI-O  • 

IFCOYT.GT.O.  ) OcTI=l./0=T 

Hl=A3*0ETT 

H2  = -#  2 *0E  T I 

H3  = A1 •OETI 

B1  = P1  »Hl«-o2«H2 

52  = Pl  * H2 ♦ °2  * H3 

8 3 =P2  *rtl*P  3*H2 

B4=P2*H2*P3*H3 

01=S*X-A1 

02 -SX  Y- A2 

03  = SY  Y - A3 

E 1=81 *01*62*02 

F2  =81  * 02  ♦ 32  * 03 

E3  =83*0 l*84  »02 

E4=83*02^34*03 

SXX=°l»El*9l4E2»B2 

SXY=P2*E1*B3*E2*B4 

SYY=P  3»E3*83>E4,B4 

Ci=SXX*SYY 

C2=SIRT((SX<-SYY)**>f4.*Sxr«*2) 

Cl=.5*(Cl»C2) 

C2=Cl -C2 

SMAj(I)=>.*SQRT(Cl) 

SMINC I)=2.*S0RT(C2) 

IFCSXY.NE.O.)  GO  TO  74 
TH( T) =0. 

Gfl  TO  5 

74  Trt(I3  =57.3*ATAN((SXX-Cl)/SXY)*90. 
5 CONTINUE 


SMOOTHER  3UTPUT 


I = OBSERVATION  [80EX 
T = TIME 

((  Si  YS)  = SMOOTiE)  POSITION  IN  X-Y  COORDINATES 
S* AJ  = SEMIMAJOP  AXIS  OF  86  PERCENT  CONTAINMENT  ELLIPSE  FOR 
SMOOTHED  POSITION 

S*  IN  * SEMIMINOR  axis  of  containment  ellipse 

TH  = ORIENTATION  op  SEMIMAJOR  AXIS  COES.  CLOCKWISE  FPflM  Y-AXIS) 

PRINT  13 
PRINY  11 

13  FORMAT! 50 X ,19H SMOOTHS)  POSITIONS,//) 

00  4 I = 1,8 

4  PRINT  7,  TC I) ,XSC l),YSC I), S^AJC I),SMlN( I),TH(I) 

5  NO 
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